{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial: CommonRoad Search \n",
    "## Graph Search Motion Planner with Motion Primitives\n",
    "\n",
    "This tutorial shows how [CommonRoad Search](https://gitlab.lrz.de/tum-cps/commonroad-search), or **Graph Search Motion Planner with Motion Primitives** is used to search for trajectories that connect an **initial state** and a **goal region**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0. Preparation\n",
    "* Before you proceed any further, make sure you have read through the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/static/docs/commonroad-io/index.html). Its tutorial can be found [here](https://commonroad.in.tum.de/tutorials/).\n",
    "* Make sure you have installed all necessary modules for CommonRoad Search according to the installation manual.\n",
    "* A documentation for the API of CommonRoad Search can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-search/index.html).\n",
    "* The API of CommonRoad-io can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Load Python Environment\n",
    "We first import necessary modules for motion planning, as well as some extensions for our convenience."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# enabling autoreload will reload modules automatically before entering the execution of code,\n",
    "# so you could edit the code for your motion planner and execute it right away without reloading again\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "\n",
    "# always show execution time for each cell\n",
    "%load_ext autotime\n",
    "\n",
    "# use notebook to get interactive plots\n",
    "%matplotlib notebook\n",
    "\n",
    "# append file paths\n",
    "import sys\n",
    "sys.path.append(\"../../GSMP/tools/commonroad-collision-checker\")\n",
    "sys.path.append(\"../../GSMP/tools/commonroad-road-boundary\")\n",
    "sys.path.append(\"../../GSMP/motion_automata\")\n",
    "\n",
    "# load necessary modules and functions\n",
    "from automata.HelperFunctions import *\n",
    "\n",
    "##################### Uncomment the following to try out exemplary motion planners #######################\n",
    "# Greedy Best First Search\n",
    "# from automata.MotionPlanner_gbfs import MotionPlanner, PriorityQueue \n",
    "\n",
    "# A-star search\n",
    "from automata.MotionPlanner_Astar import MotionPlanner, PriorityQueue\n",
    "\n",
    "##################### Uncomment the following to load your own motion planner      #######################\n",
    "# from automata.MotionPlanner import MotionPlanner, PriorityQueue"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Load Scenarios\n",
    "You may uncomment the scenario in which you want to plan the trajectory. Scenarios beginning with letter C stand for cooeprative driving scenarios and possess multiple planning problems. In this case you should provide solutions to all planning problems."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "application/javascript": [
       "/* Put everything inside the global mpl namespace */\n",
       "window.mpl = {};\n",
       "\n",
       "\n",
       "mpl.get_websocket_type = function() {\n",
       "    if (typeof(WebSocket) !== 'undefined') {\n",
       "        return WebSocket;\n",
       "    } else if (typeof(MozWebSocket) !== 'undefined') {\n",
       "        return MozWebSocket;\n",
       "    } else {\n",
       "        alert('Your browser does not have WebSocket support. ' +\n",
       "              'Please try Chrome, Safari or Firefox ≥ 6. ' +\n",
       "              'Firefox 4 and 5 are also supported but you ' +\n",
       "              'have to enable WebSockets in about:config.');\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure = function(figure_id, websocket, ondownload, parent_element) {\n",
       "    this.id = figure_id;\n",
       "\n",
       "    this.ws = websocket;\n",
       "\n",
       "    this.supports_binary = (this.ws.binaryType != undefined);\n",
       "\n",
       "    if (!this.supports_binary) {\n",
       "        var warnings = document.getElementById(\"mpl-warnings\");\n",
       "        if (warnings) {\n",
       "            warnings.style.display = 'block';\n",
       "            warnings.textContent = (\n",
       "                \"This browser does not support binary websocket messages. \" +\n",
       "                    \"Performance may be slow.\");\n",
       "        }\n",
       "    }\n",
       "\n",
       "    this.imageObj = new Image();\n",
       "\n",
       "    this.context = undefined;\n",
       "    this.message = undefined;\n",
       "    this.canvas = undefined;\n",
       "    this.rubberband_canvas = undefined;\n",
       "    this.rubberband_context = undefined;\n",
       "    this.format_dropdown = undefined;\n",
       "\n",
       "    this.image_mode = 'full';\n",
       "\n",
       "    this.root = $('<div/>');\n",
       "    this._root_extra_style(this.root)\n",
       "    this.root.attr('style', 'display: inline-block');\n",
       "\n",
       "    $(parent_element).append(this.root);\n",
       "\n",
       "    this._init_header(this);\n",
       "    this._init_canvas(this);\n",
       "    this._init_toolbar(this);\n",
       "\n",
       "    var fig = this;\n",
       "\n",
       "    this.waiting = false;\n",
       "\n",
       "    this.ws.onopen =  function () {\n",
       "            fig.send_message(\"supports_binary\", {value: fig.supports_binary});\n",
       "            fig.send_message(\"send_image_mode\", {});\n",
       "            if (mpl.ratio != 1) {\n",
       "                fig.send_message(\"set_dpi_ratio\", {'dpi_ratio': mpl.ratio});\n",
       "            }\n",
       "            fig.send_message(\"refresh\", {});\n",
       "        }\n",
       "\n",
       "    this.imageObj.onload = function() {\n",
       "            if (fig.image_mode == 'full') {\n",
       "                // Full images could contain transparency (where diff images\n",
       "                // almost always do), so we need to clear the canvas so that\n",
       "                // there is no ghosting.\n",
       "                fig.context.clearRect(0, 0, fig.canvas.width, fig.canvas.height);\n",
       "            }\n",
       "            fig.context.drawImage(fig.imageObj, 0, 0);\n",
       "        };\n",
       "\n",
       "    this.imageObj.onunload = function() {\n",
       "        fig.ws.close();\n",
       "    }\n",
       "\n",
       "    this.ws.onmessage = this._make_on_message_function(this);\n",
       "\n",
       "    this.ondownload = ondownload;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_header = function() {\n",
       "    var titlebar = $(\n",
       "        '<div class=\"ui-dialog-titlebar ui-widget-header ui-corner-all ' +\n",
       "        'ui-helper-clearfix\"/>');\n",
       "    var titletext = $(\n",
       "        '<div class=\"ui-dialog-title\" style=\"width: 100%; ' +\n",
       "        'text-align: center; padding: 3px;\"/>');\n",
       "    titlebar.append(titletext)\n",
       "    this.root.append(titlebar);\n",
       "    this.header = titletext[0];\n",
       "}\n",
       "\n",
       "\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_canvas = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var canvas_div = $('<div/>');\n",
       "\n",
       "    canvas_div.attr('style', 'position: relative; clear: both; outline: 0');\n",
       "\n",
       "    function canvas_keyboard_event(event) {\n",
       "        return fig.key_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    canvas_div.keydown('key_press', canvas_keyboard_event);\n",
       "    canvas_div.keyup('key_release', canvas_keyboard_event);\n",
       "    this.canvas_div = canvas_div\n",
       "    this._canvas_extra_style(canvas_div)\n",
       "    this.root.append(canvas_div);\n",
       "\n",
       "    var canvas = $('<canvas/>');\n",
       "    canvas.addClass('mpl-canvas');\n",
       "    canvas.attr('style', \"left: 0; top: 0; z-index: 0; outline: 0\")\n",
       "\n",
       "    this.canvas = canvas[0];\n",
       "    this.context = canvas[0].getContext(\"2d\");\n",
       "\n",
       "    var backingStore = this.context.backingStorePixelRatio ||\n",
       "\tthis.context.webkitBackingStorePixelRatio ||\n",
       "\tthis.context.mozBackingStorePixelRatio ||\n",
       "\tthis.context.msBackingStorePixelRatio ||\n",
       "\tthis.context.oBackingStorePixelRatio ||\n",
       "\tthis.context.backingStorePixelRatio || 1;\n",
       "\n",
       "    mpl.ratio = (window.devicePixelRatio || 1) / backingStore;\n",
       "\n",
       "    var rubberband = $('<canvas/>');\n",
       "    rubberband.attr('style', \"position: absolute; left: 0; top: 0; z-index: 1;\")\n",
       "\n",
       "    var pass_mouse_events = true;\n",
       "\n",
       "    canvas_div.resizable({\n",
       "        start: function(event, ui) {\n",
       "            pass_mouse_events = false;\n",
       "        },\n",
       "        resize: function(event, ui) {\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "        stop: function(event, ui) {\n",
       "            pass_mouse_events = true;\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "    });\n",
       "\n",
       "    function mouse_event_fn(event) {\n",
       "        if (pass_mouse_events)\n",
       "            return fig.mouse_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    rubberband.mousedown('button_press', mouse_event_fn);\n",
       "    rubberband.mouseup('button_release', mouse_event_fn);\n",
       "    // Throttle sequential mouse events to 1 every 20ms.\n",
       "    rubberband.mousemove('motion_notify', mouse_event_fn);\n",
       "\n",
       "    rubberband.mouseenter('figure_enter', mouse_event_fn);\n",
       "    rubberband.mouseleave('figure_leave', mouse_event_fn);\n",
       "\n",
       "    canvas_div.on(\"wheel\", function (event) {\n",
       "        event = event.originalEvent;\n",
       "        event['data'] = 'scroll'\n",
       "        if (event.deltaY < 0) {\n",
       "            event.step = 1;\n",
       "        } else {\n",
       "            event.step = -1;\n",
       "        }\n",
       "        mouse_event_fn(event);\n",
       "    });\n",
       "\n",
       "    canvas_div.append(canvas);\n",
       "    canvas_div.append(rubberband);\n",
       "\n",
       "    this.rubberband = rubberband;\n",
       "    this.rubberband_canvas = rubberband[0];\n",
       "    this.rubberband_context = rubberband[0].getContext(\"2d\");\n",
       "    this.rubberband_context.strokeStyle = \"#000000\";\n",
       "\n",
       "    this._resize_canvas = function(width, height) {\n",
       "        // Keep the size of the canvas, canvas container, and rubber band\n",
       "        // canvas in synch.\n",
       "        canvas_div.css('width', width)\n",
       "        canvas_div.css('height', height)\n",
       "\n",
       "        canvas.attr('width', width * mpl.ratio);\n",
       "        canvas.attr('height', height * mpl.ratio);\n",
       "        canvas.attr('style', 'width: ' + width + 'px; height: ' + height + 'px;');\n",
       "\n",
       "        rubberband.attr('width', width);\n",
       "        rubberband.attr('height', height);\n",
       "    }\n",
       "\n",
       "    // Set the figure to an initial 600x600px, this will subsequently be updated\n",
       "    // upon first draw.\n",
       "    this._resize_canvas(600, 600);\n",
       "\n",
       "    // Disable right mouse context menu.\n",
       "    $(this.rubberband_canvas).bind(\"contextmenu\",function(e){\n",
       "        return false;\n",
       "    });\n",
       "\n",
       "    function set_focus () {\n",
       "        canvas.focus();\n",
       "        canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    window.setTimeout(set_focus, 100);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>');\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items) {\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) {\n",
       "            // put a spacer in here.\n",
       "            continue;\n",
       "        }\n",
       "        var button = $('<button/>');\n",
       "        button.addClass('ui-button ui-widget ui-state-default ui-corner-all ' +\n",
       "                        'ui-button-icon-only');\n",
       "        button.attr('role', 'button');\n",
       "        button.attr('aria-disabled', 'false');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "\n",
       "        var icon_img = $('<span/>');\n",
       "        icon_img.addClass('ui-button-icon-primary ui-icon');\n",
       "        icon_img.addClass(image);\n",
       "        icon_img.addClass('ui-corner-all');\n",
       "\n",
       "        var tooltip_span = $('<span/>');\n",
       "        tooltip_span.addClass('ui-button-text');\n",
       "        tooltip_span.html(tooltip);\n",
       "\n",
       "        button.append(icon_img);\n",
       "        button.append(tooltip_span);\n",
       "\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    var fmt_picker_span = $('<span/>');\n",
       "\n",
       "    var fmt_picker = $('<select/>');\n",
       "    fmt_picker.addClass('mpl-toolbar-option ui-widget ui-widget-content');\n",
       "    fmt_picker_span.append(fmt_picker);\n",
       "    nav_element.append(fmt_picker_span);\n",
       "    this.format_dropdown = fmt_picker[0];\n",
       "\n",
       "    for (var ind in mpl.extensions) {\n",
       "        var fmt = mpl.extensions[ind];\n",
       "        var option = $(\n",
       "            '<option/>', {selected: fmt === mpl.default_extension}).html(fmt);\n",
       "        fmt_picker.append(option);\n",
       "    }\n",
       "\n",
       "    // Add hover states to the ui-buttons\n",
       "    $( \".ui-button\" ).hover(\n",
       "        function() { $(this).addClass(\"ui-state-hover\");},\n",
       "        function() { $(this).removeClass(\"ui-state-hover\");}\n",
       "    );\n",
       "\n",
       "    var status_bar = $('<span class=\"mpl-message\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.request_resize = function(x_pixels, y_pixels) {\n",
       "    // Request matplotlib to resize the figure. Matplotlib will then trigger a resize in the client,\n",
       "    // which will in turn request a refresh of the image.\n",
       "    this.send_message('resize', {'width': x_pixels, 'height': y_pixels});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_message = function(type, properties) {\n",
       "    properties['type'] = type;\n",
       "    properties['figure_id'] = this.id;\n",
       "    this.ws.send(JSON.stringify(properties));\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_draw_message = function() {\n",
       "    if (!this.waiting) {\n",
       "        this.waiting = true;\n",
       "        this.ws.send(JSON.stringify({type: \"draw\", figure_id: this.id}));\n",
       "    }\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    var format_dropdown = fig.format_dropdown;\n",
       "    var format = format_dropdown.options[format_dropdown.selectedIndex].value;\n",
       "    fig.ondownload(fig, format);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_resize = function(fig, msg) {\n",
       "    var size = msg['size'];\n",
       "    if (size[0] != fig.canvas.width || size[1] != fig.canvas.height) {\n",
       "        fig._resize_canvas(size[0], size[1]);\n",
       "        fig.send_message(\"refresh\", {});\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_rubberband = function(fig, msg) {\n",
       "    var x0 = msg['x0'] / mpl.ratio;\n",
       "    var y0 = (fig.canvas.height - msg['y0']) / mpl.ratio;\n",
       "    var x1 = msg['x1'] / mpl.ratio;\n",
       "    var y1 = (fig.canvas.height - msg['y1']) / mpl.ratio;\n",
       "    x0 = Math.floor(x0) + 0.5;\n",
       "    y0 = Math.floor(y0) + 0.5;\n",
       "    x1 = Math.floor(x1) + 0.5;\n",
       "    y1 = Math.floor(y1) + 0.5;\n",
       "    var min_x = Math.min(x0, x1);\n",
       "    var min_y = Math.min(y0, y1);\n",
       "    var width = Math.abs(x1 - x0);\n",
       "    var height = Math.abs(y1 - y0);\n",
       "\n",
       "    fig.rubberband_context.clearRect(\n",
       "        0, 0, fig.canvas.width / mpl.ratio, fig.canvas.height / mpl.ratio);\n",
       "\n",
       "    fig.rubberband_context.strokeRect(min_x, min_y, width, height);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_figure_label = function(fig, msg) {\n",
       "    // Updates the figure title.\n",
       "    fig.header.textContent = msg['label'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_cursor = function(fig, msg) {\n",
       "    var cursor = msg['cursor'];\n",
       "    switch(cursor)\n",
       "    {\n",
       "    case 0:\n",
       "        cursor = 'pointer';\n",
       "        break;\n",
       "    case 1:\n",
       "        cursor = 'default';\n",
       "        break;\n",
       "    case 2:\n",
       "        cursor = 'crosshair';\n",
       "        break;\n",
       "    case 3:\n",
       "        cursor = 'move';\n",
       "        break;\n",
       "    }\n",
       "    fig.rubberband_canvas.style.cursor = cursor;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_message = function(fig, msg) {\n",
       "    fig.message.textContent = msg['message'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_draw = function(fig, msg) {\n",
       "    // Request the server to send over a new figure.\n",
       "    fig.send_draw_message();\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_image_mode = function(fig, msg) {\n",
       "    fig.image_mode = msg['mode'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Called whenever the canvas gets updated.\n",
       "    this.send_message(\"ack\", {});\n",
       "}\n",
       "\n",
       "// A function to construct a web socket function for onmessage handling.\n",
       "// Called in the figure constructor.\n",
       "mpl.figure.prototype._make_on_message_function = function(fig) {\n",
       "    return function socket_on_message(evt) {\n",
       "        if (evt.data instanceof Blob) {\n",
       "            /* FIXME: We get \"Resource interpreted as Image but\n",
       "             * transferred with MIME type text/plain:\" errors on\n",
       "             * Chrome.  But how to set the MIME type?  It doesn't seem\n",
       "             * to be part of the websocket stream */\n",
       "            evt.data.type = \"image/png\";\n",
       "\n",
       "            /* Free the memory for the previous frames */\n",
       "            if (fig.imageObj.src) {\n",
       "                (window.URL || window.webkitURL).revokeObjectURL(\n",
       "                    fig.imageObj.src);\n",
       "            }\n",
       "\n",
       "            fig.imageObj.src = (window.URL || window.webkitURL).createObjectURL(\n",
       "                evt.data);\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "        else if (typeof evt.data === 'string' && evt.data.slice(0, 21) == \"data:image/png;base64\") {\n",
       "            fig.imageObj.src = evt.data;\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        var msg = JSON.parse(evt.data);\n",
       "        var msg_type = msg['type'];\n",
       "\n",
       "        // Call the  \"handle_{type}\" callback, which takes\n",
       "        // the figure and JSON message as its only arguments.\n",
       "        try {\n",
       "            var callback = fig[\"handle_\" + msg_type];\n",
       "        } catch (e) {\n",
       "            console.log(\"No handler for the '\" + msg_type + \"' message type: \", msg);\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        if (callback) {\n",
       "            try {\n",
       "                // console.log(\"Handling '\" + msg_type + \"' message: \", msg);\n",
       "                callback(fig, msg);\n",
       "            } catch (e) {\n",
       "                console.log(\"Exception inside the 'handler_\" + msg_type + \"' callback:\", e, e.stack, msg);\n",
       "            }\n",
       "        }\n",
       "    };\n",
       "}\n",
       "\n",
       "// from http://stackoverflow.com/questions/1114465/getting-mouse-location-in-canvas\n",
       "mpl.findpos = function(e) {\n",
       "    //this section is from http://www.quirksmode.org/js/events_properties.html\n",
       "    var targ;\n",
       "    if (!e)\n",
       "        e = window.event;\n",
       "    if (e.target)\n",
       "        targ = e.target;\n",
       "    else if (e.srcElement)\n",
       "        targ = e.srcElement;\n",
       "    if (targ.nodeType == 3) // defeat Safari bug\n",
       "        targ = targ.parentNode;\n",
       "\n",
       "    // jQuery normalizes the pageX and pageY\n",
       "    // pageX,Y are the mouse positions relative to the document\n",
       "    // offset() returns the position of the element relative to the document\n",
       "    var x = e.pageX - $(targ).offset().left;\n",
       "    var y = e.pageY - $(targ).offset().top;\n",
       "\n",
       "    return {\"x\": x, \"y\": y};\n",
       "};\n",
       "\n",
       "/*\n",
       " * return a copy of an object with only non-object keys\n",
       " * we need this to avoid circular references\n",
       " * http://stackoverflow.com/a/24161582/3208463\n",
       " */\n",
       "function simpleKeys (original) {\n",
       "  return Object.keys(original).reduce(function (obj, key) {\n",
       "    if (typeof original[key] !== 'object')\n",
       "        obj[key] = original[key]\n",
       "    return obj;\n",
       "  }, {});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.mouse_event = function(event, name) {\n",
       "    var canvas_pos = mpl.findpos(event)\n",
       "\n",
       "    if (name === 'button_press')\n",
       "    {\n",
       "        this.canvas.focus();\n",
       "        this.canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    var x = canvas_pos.x * mpl.ratio;\n",
       "    var y = canvas_pos.y * mpl.ratio;\n",
       "\n",
       "    this.send_message(name, {x: x, y: y, button: event.button,\n",
       "                             step: event.step,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "\n",
       "    /* This prevents the web browser from automatically changing to\n",
       "     * the text insertion cursor when the button is pressed.  We want\n",
       "     * to control all of the cursor setting manually through the\n",
       "     * 'cursor' event from matplotlib */\n",
       "    event.preventDefault();\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    // Handle any extra behaviour associated with a key event\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.key_event = function(event, name) {\n",
       "\n",
       "    // Prevent repeat events\n",
       "    if (name == 'key_press')\n",
       "    {\n",
       "        if (event.which === this._key)\n",
       "            return;\n",
       "        else\n",
       "            this._key = event.which;\n",
       "    }\n",
       "    if (name == 'key_release')\n",
       "        this._key = null;\n",
       "\n",
       "    var value = '';\n",
       "    if (event.ctrlKey && event.which != 17)\n",
       "        value += \"ctrl+\";\n",
       "    if (event.altKey && event.which != 18)\n",
       "        value += \"alt+\";\n",
       "    if (event.shiftKey && event.which != 16)\n",
       "        value += \"shift+\";\n",
       "\n",
       "    value += 'k';\n",
       "    value += event.which.toString();\n",
       "\n",
       "    this._key_event_extra(event, name);\n",
       "\n",
       "    this.send_message(name, {key: value,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onclick = function(name) {\n",
       "    if (name == 'download') {\n",
       "        this.handle_save(this, null);\n",
       "    } else {\n",
       "        this.send_message(\"toolbar_button\", {name: name});\n",
       "    }\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onmouseover = function(tooltip) {\n",
       "    this.message.textContent = tooltip;\n",
       "};\n",
       "mpl.toolbar_items = [[\"Home\", \"Reset original view\", \"fa fa-home icon-home\", \"home\"], [\"Back\", \"Back to previous view\", \"fa fa-arrow-left icon-arrow-left\", \"back\"], [\"Forward\", \"Forward to next view\", \"fa fa-arrow-right icon-arrow-right\", \"forward\"], [\"\", \"\", \"\", \"\"], [\"Pan\", \"Pan axes with left mouse, zoom with right\", \"fa fa-arrows icon-move\", \"pan\"], [\"Zoom\", \"Zoom to rectangle\", \"fa fa-square-o icon-check-empty\", \"zoom\"], [\"\", \"\", \"\", \"\"], [\"Download\", \"Download plot\", \"fa fa-floppy-o icon-save\", \"download\"]];\n",
       "\n",
       "mpl.extensions = [\"pdf\", \"eps\", \"png\", \"ps\", \"raw\", \"svg\"];\n",
       "\n",
       "mpl.default_extension = \"png\";var comm_websocket_adapter = function(comm) {\n",
       "    // Create a \"websocket\"-like object which calls the given IPython comm\n",
       "    // object with the appropriate methods. Currently this is a non binary\n",
       "    // socket, so there is still some room for performance tuning.\n",
       "    var ws = {};\n",
       "\n",
       "    ws.close = function() {\n",
       "        comm.close()\n",
       "    };\n",
       "    ws.send = function(m) {\n",
       "        //console.log('sending', m);\n",
       "        comm.send(m);\n",
       "    };\n",
       "    // Register the callback with on_msg.\n",
       "    comm.on_msg(function(msg) {\n",
       "        //console.log('receiving', msg['content']['data'], msg);\n",
       "        // Pass the mpl event to the overridden (by mpl) onmessage function.\n",
       "        ws.onmessage(msg['content']['data'])\n",
       "    });\n",
       "    return ws;\n",
       "}\n",
       "\n",
       "mpl.mpl_figure_comm = function(comm, msg) {\n",
       "    // This is the function which gets called when the mpl process\n",
       "    // starts-up an IPython Comm through the \"matplotlib\" channel.\n",
       "\n",
       "    var id = msg.content.data.id;\n",
       "    // Get hold of the div created by the display call when the Comm\n",
       "    // socket was opened in Python.\n",
       "    var element = $(\"#\" + id);\n",
       "    var ws_proxy = comm_websocket_adapter(comm)\n",
       "\n",
       "    function ondownload(figure, format) {\n",
       "        window.open(figure.imageObj.src);\n",
       "    }\n",
       "\n",
       "    var fig = new mpl.figure(id, ws_proxy,\n",
       "                           ondownload,\n",
       "                           element.get(0));\n",
       "\n",
       "    // Call onopen now - mpl needs it, as it is assuming we've passed it a real\n",
       "    // web socket which is closed, not our websocket->open comm proxy.\n",
       "    ws_proxy.onopen();\n",
       "\n",
       "    fig.parent_element = element.get(0);\n",
       "    fig.cell_info = mpl.find_output_cell(\"<div id='\" + id + \"'></div>\");\n",
       "    if (!fig.cell_info) {\n",
       "        console.error(\"Failed to find cell for figure\", id, fig);\n",
       "        return;\n",
       "    }\n",
       "\n",
       "    var output_index = fig.cell_info[2]\n",
       "    var cell = fig.cell_info[0];\n",
       "\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.handle_close = function(fig, msg) {\n",
       "    var width = fig.canvas.width/mpl.ratio\n",
       "    fig.root.unbind('remove')\n",
       "\n",
       "    // Update the output cell to use the data from the current canvas.\n",
       "    fig.push_to_output();\n",
       "    var dataURL = fig.canvas.toDataURL();\n",
       "    // Re-enable the keyboard manager in IPython - without this line, in FF,\n",
       "    // the notebook keyboard shortcuts fail.\n",
       "    IPython.keyboard_manager.enable()\n",
       "    $(fig.parent_element).html('<img src=\"' + dataURL + '\" width=\"' + width + '\">');\n",
       "    fig.close_ws(fig, msg);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.close_ws = function(fig, msg){\n",
       "    fig.send_message('closing', msg);\n",
       "    // fig.ws.close()\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.push_to_output = function(remove_interactive) {\n",
       "    // Turn the data on the canvas into data in the output cell.\n",
       "    var width = this.canvas.width/mpl.ratio\n",
       "    var dataURL = this.canvas.toDataURL();\n",
       "    this.cell_info[1]['text/html'] = '<img src=\"' + dataURL + '\" width=\"' + width + '\">';\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Tell IPython that the notebook contents must change.\n",
       "    IPython.notebook.set_dirty(true);\n",
       "    this.send_message(\"ack\", {});\n",
       "    var fig = this;\n",
       "    // Wait a second, then push the new image to the DOM so\n",
       "    // that it is saved nicely (might be nice to debounce this).\n",
       "    setTimeout(function () { fig.push_to_output() }, 1000);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>');\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items){\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) { continue; };\n",
       "\n",
       "        var button = $('<button class=\"btn btn-default\" href=\"#\" title=\"' + name + '\"><i class=\"fa ' + image + ' fa-lg\"></i></button>');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    // Add the status bar.\n",
       "    var status_bar = $('<span class=\"mpl-message\" style=\"text-align:right; float: right;\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "\n",
       "    // Add the close button to the window.\n",
       "    var buttongrp = $('<div class=\"btn-group inline pull-right\"></div>');\n",
       "    var button = $('<button class=\"btn btn-mini btn-primary\" href=\"#\" title=\"Stop Interaction\"><i class=\"fa fa-power-off icon-remove icon-large\"></i></button>');\n",
       "    button.click(function (evt) { fig.handle_close(fig, {}); } );\n",
       "    button.mouseover('Stop Interaction', toolbar_mouse_event);\n",
       "    buttongrp.append(button);\n",
       "    var titlebar = this.root.find($('.ui-dialog-titlebar'));\n",
       "    titlebar.prepend(buttongrp);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(el){\n",
       "    var fig = this\n",
       "    el.on(\"remove\", function(){\n",
       "\tfig.close_ws(fig, {});\n",
       "    });\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(el){\n",
       "    // this is important to make the div 'focusable\n",
       "    el.attr('tabindex', 0)\n",
       "    // reach out to IPython and tell the keyboard manager to turn it's self\n",
       "    // off when our div gets focus\n",
       "\n",
       "    // location in version 3\n",
       "    if (IPython.notebook.keyboard_manager) {\n",
       "        IPython.notebook.keyboard_manager.register_events(el);\n",
       "    }\n",
       "    else {\n",
       "        // location in version 2\n",
       "        IPython.keyboard_manager.register_events(el);\n",
       "    }\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    var manager = IPython.notebook.keyboard_manager;\n",
       "    if (!manager)\n",
       "        manager = IPython.keyboard_manager;\n",
       "\n",
       "    // Check for shift+enter\n",
       "    if (event.shiftKey && event.which == 13) {\n",
       "        this.canvas_div.blur();\n",
       "        event.shiftKey = false;\n",
       "        // Send a \"J\" for go to next cell\n",
       "        event.which = 74;\n",
       "        event.keyCode = 74;\n",
       "        manager.command_mode();\n",
       "        manager.handle_keydown(event);\n",
       "    }\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    fig.ondownload(fig, null);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.find_output_cell = function(html_output) {\n",
       "    // Return the cell and output element which can be found *uniquely* in the notebook.\n",
       "    // Note - this is a bit hacky, but it is done because the \"notebook_saving.Notebook\"\n",
       "    // IPython event is triggered only after the cells have been serialised, which for\n",
       "    // our purposes (turning an active figure into a static one), is too late.\n",
       "    var cells = IPython.notebook.get_cells();\n",
       "    var ncells = cells.length;\n",
       "    for (var i=0; i<ncells; i++) {\n",
       "        var cell = cells[i];\n",
       "        if (cell.cell_type === 'code'){\n",
       "            for (var j=0; j<cell.output_area.outputs.length; j++) {\n",
       "                var data = cell.output_area.outputs[j];\n",
       "                if (data.data) {\n",
       "                    // IPython >= 3 moved mimebundle to data attribute of output\n",
       "                    data = data.data;\n",
       "                }\n",
       "                if (data['text/html'] == html_output) {\n",
       "                    return [cell, data, j];\n",
       "                }\n",
       "            }\n",
       "        }\n",
       "    }\n",
       "}\n",
       "\n",
       "// Register the function which deals with the matplotlib target/channel.\n",
       "// The kernel may be null if the page has been refreshed.\n",
       "if (IPython.notebook.kernel != null) {\n",
       "    IPython.notebook.kernel.comm_manager.register_target('matplotlib', mpl.mpl_figure_comm);\n",
       "}\n"
      ],
      "text/plain": [
       "<IPython.core.display.Javascript object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/html": [
       "<img src=\"\" width=\"799.2307985463802\">"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "time: 379 ms\n"
     ]
    }
   ],
   "source": [
    "scenario_path_prefix = '../../scenarios/'\n",
    "\n",
    "### easy scenarios for testing ###\n",
    "# scenario_id = 'USA_Lanker-1_6_T-1'\n",
    "# scenario_id = 'USA_Lanker-1_9_T-1'\n",
    "# scenario_id = 'USA_Lanker-1_11_T-1'\n",
    "\n",
    "###         harder ones        ###\n",
    "scenario_id = 'USA_Lanker-1_2_T-1' \n",
    "# scenario_id = 'USA_Lanker-2_15_T-1'\n",
    "# scenario_id = 'USA_US101-2_1_T-1' \n",
    "# scenario_id = 'USA_US101-7_1_T-1' \n",
    "# scenario_id = 'USA_US101-9_1_T-1' \n",
    "# scenario_id = 'USA_US101-10_1_T-1'\n",
    "# scenario_id = 'USA_US101-12_1_T-1' \n",
    "# scenario_id = 'USA_US101-15_1_T-1' \n",
    "# scenario_id = 'USA_US101-18_1_T-1' \n",
    "# scenario_id = 'USA_US101-19_1_T-1' \n",
    "# scenario_id = 'DEU_A9-1_1_T-1' \n",
    "# scenario_id = 'C-DEU_B471-2_1' \n",
    "\n",
    "# construct file path\n",
    "scenario_path = scenario_path_prefix + scenario_id + '.xml'\n",
    "\n",
    "# open and read in scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(scenario_path).open()\n",
    "\n",
    "# plot the scenario and planning problem set\n",
    "plt.figure(figsize=(8, 8))\n",
    "draw_object(scenario)\n",
    "draw_object(planning_problem_set)\n",
    "plt.gca().set_aspect('equal')\n",
    "plt.margins(0,0)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Load Motion Primitives\n",
    "We call generate_automata() to generate an automata instance. This might take up to a minute."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reading motion primitives...\n",
      "Automata created.\n",
      "Loaded primitives: 2984\n",
      "time: 5.65 s\n"
     ]
    }
   ],
   "source": [
    "automata = generate_automata(scenario.dt)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "42\n",
      "time: 180 ms\n"
     ]
    }
   ],
   "source": [
    "for primitive in automata.Primitives:\n",
    "    print(len(primitive.trajectory.state_list))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Initialize Motion Planner\n",
    "\n",
    "In this step, we would like to configure the planning problem for the motion planner. Each **planning problem** has a individual **initial state** and a **goal region**. The **initial state** and **goal region** should be passed into the motion planner to start searching for a solution.\n",
    "\n",
    "Note: the shape of the ego vehicle should also be defined for collision checking to work properly"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Initial Sate: \n",
      "position= [0. 0.]\n",
      "orientation= -2.0334\n",
      "velocity= 5.8461\n",
      "yaw_rate= -0.002611\n",
      "slip_angle= -0.010302\n",
      "time_step= 0\n",
      "\n",
      "Goal Region: \n",
      "position= Rectangle: \n",
      "\t width: 1.9652 \n",
      "\t length: 2.5548 \n",
      "\t center: [ 10.4999 -30.0238] \n",
      "\t orientation: -0.48197 \n",
      "\n",
      "orientation= Interval\n",
      "start: -0.58392\n",
      "end: -0.40939\n",
      "\n",
      "velocity= Interval\n",
      "start: 5.9794\n",
      "end: 11.9794\n",
      "\n",
      "time_step= Interval\n",
      "start: 44\n",
      "end: 54\n",
      "\n",
      "\n",
      "time: 307 ms\n"
     ]
    }
   ],
   "source": [
    "# retrieve planning problem with given index (cooperative scenario:0, 1, 2, ..., else: 0)\n",
    "planning_problem_idx = 0\n",
    "planning_problem = list(planning_problem_set.planning_problem_dict.values())[planning_problem_idx]\n",
    "\n",
    "# add initial state of planning problem into automata\n",
    "automata, initial_motion_primitive = add_initial_state_to_automata(automata, planning_problem)\n",
    "\n",
    "# construct motion planner. Note: ego_vehicle_shape already loaded in the helperfunction in the initialization\n",
    "motion_planner = MotionPlanner(scenario, planning_problem, automata, ego_vehicle_shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Check Feasibility of Raw Motion Primitives （with Solution Checker)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "time: 3.41 ms\n"
     ]
    }
   ],
   "source": [
    "def create_new_traj_KS(primitive):\n",
    "    # retrieve states of the primitive: start + 40 states of trajectory + final\n",
    "    state_start_primitive = primitive.startState\n",
    "    state_final_primitive = primitive.finalState   \n",
    "    list_states_primitive = primitive.trajectory.state_list\n",
    "    \n",
    "    list_states_new = list()\n",
    " \n",
    "    # note: in state_start, initial x and y position is not very clear from the given\n",
    "    # primitive file. I have omitted the start state for the time being, so here I only\n",
    "    # evaluate the trajectory states + final state.\n",
    "    \n",
    "    # trajectory states\n",
    "    for state in list_states_primitive:\n",
    "        \n",
    "        # feed in required slots\n",
    "        kwarg = {'position': state.position,\n",
    "                 'velocity': state.velocity,\n",
    "                 'steering_angle': 0, # does not matter here, as it is not checked in the solution checker\n",
    "                 'orientation': state.orientation,\n",
    "                 'time_step': state.time_step}\n",
    "\n",
    "        # append state\n",
    "        list_states_new.append(State(**kwarg))         \n",
    "\n",
    "    # final state    \n",
    "    kwarg = {'position': np.array([state_final_primitive.x, state_final_primitive.y]),\n",
    "             'velocity': state_final_primitive.velocity,\n",
    "             'steering_angle': 0,\n",
    "             'orientation': state_final_primitive.orientation,\n",
    "             'time_step': 10} # final state appears at time = 1 seconds = time_step 10 (with scenario.dt = 0.1)\n",
    "\n",
    "    state_final = State(**kwarg)\n",
    "    list_states_new.append(state_final)\n",
    "    \n",
    "    # create new trajectory for evaluation\n",
    "    trajectory_new = Trajectory(initial_time_step=list_states_new[0].time_step, state_list=list_states_new)\n",
    "    \n",
    "    return trajectory_new\n",
    "        \n",
    "def create_new_traj_PM(primitive):\n",
    "    # retrieve states of the primitive: start + 40 states of trajectory + final\n",
    "    state_start_primitive = primitive.startState\n",
    "    state_final_primitive = primitive.finalState   \n",
    "    list_states_primitive = primitive.trajectory.state_list\n",
    "    \n",
    "    list_states_new = list()\n",
    "    \n",
    "    # note: in state_start, initial x and y position is not very clear from the given\n",
    "    # primitive file. I have omitted the start state for the time being, so here I only\n",
    "    # evaluate the trajectory states + final state.\n",
    "    \n",
    "    # trajectory states\n",
    "    for state in list_states_primitive:\n",
    "        # decompose velocity into v_x and v_y\n",
    "        v_x = state.velocity * cos(state.orientation)\n",
    "        v_y = state.velocity * sin(state.orientation)\n",
    "        \n",
    "        # feed in required slots\n",
    "        kwarg = {'position': state.position, \n",
    "                 'velocity': v_x,\n",
    "                 'velocity_y': v_y,\n",
    "                 'orientation': state.orientation, \n",
    "                 'time_step': state.time_step}\n",
    "        # append state\n",
    "        list_states_new.append(State(**kwarg))\n",
    "    \n",
    "    # final state    \n",
    "    kwarg = {'position': np.array([state_final_primitive.x, state_final_primitive.y]),\n",
    "             'velocity': state_final_primitive.velocity * cos(state_final_primitive.orientation),\n",
    "             'velocity_y': state_final_primitive.velocity * sin(state_final_primitive.orientation),\n",
    "             'orientation': state_final_primitive.orientation,\n",
    "             'time_step': 10} # final state appears at time = 1 seconds = time_step 10 (with scenario.dt = 0.1)\n",
    "    \n",
    "    state_final = State(**kwarg)\n",
    "    list_states_new.append(state_final) \n",
    "    \n",
    "    # create new trajectory for evaluation\n",
    "    trajectory_new = Trajectory(initial_time_step=list_states_new[0].time_step, state_list=list_states_new)\n",
    "    \n",
    "    return trajectory_new\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Checking feasibility with PM model, vehicle id: 3.\n",
      "Total number of primitives to be checked:  50\n",
      "Feasible percentage: 100  count:   1\n",
      "Feasible percentage:  90  count:  12\n",
      "Feasible percentage:  80  count:  18\n",
      "Feasible percentage:  70  count:   0\n",
      "Feasible percentage:  60  count:   0\n",
      "Feasible percentage:  50  count:   0\n",
      "Feasible percentage:  40  count:   0\n",
      "Feasible percentage:  30  count:   0\n",
      "Feasible percentage:  20  count:   0\n",
      "Feasible percentage:  10  count:   0\n",
      "Feasible percentage:   0  count:  19\n",
      "time: 6.81 s\n"
     ]
    }
   ],
   "source": [
    "# add path to vehicle model (https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/tree/master/Python)\n",
    "sys.path.append(\"/home/edmond/Softwares/commonroad/commonroad-search/GSMP/motion_automata/vehicle_model\")\n",
    "\n",
    "from solution_checker import *\n",
    "import xml.etree.ElementTree as et\n",
    "from math import atan2, sin, cos\n",
    "from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction\n",
    "\n",
    "# 0: KS 1:PM\n",
    "mode = 1\n",
    "\n",
    "# 1: FORD_ESCORT 2: BMW_320i 3: VW_VANAGON\n",
    "veh_type_id = 3\n",
    "\n",
    "if veh_type_id == 1:\n",
    "    veh_type = VehicleType.FORD_ESCORT\n",
    "elif veh_type_id == 2:\n",
    "    veh_type = VehicleType.BMW_320i\n",
    "elif veh_type_id == 3:\n",
    "    veh_type = VehicleType.VW_VANAGON\n",
    "    \n",
    "# for feasible percentage computation\n",
    "dict_feasible_percentage = {}\n",
    "list_percentage = [p for p in range(0, 101, 10)]\n",
    "list_percentage.reverse()\n",
    "for percentage in list_percentage:\n",
    "    dict_feasible_percentage[percentage] = 0\n",
    "\n",
    "if not mode:\n",
    "    print(\"Checking feasibility with KS model, vehicle id: {}.\".format(veh_type_id))\n",
    "else:\n",
    "    print(\"Checking feasibility with PM model, vehicle id: {}.\".format(veh_type_id))\n",
    "    \n",
    "num_prim_to_be_checked = len(automata.Primitives)\n",
    "num_prim_to_be_checked = 50\n",
    "\n",
    "print(\"Total number of primitives to be checked: \", num_prim_to_be_checked)\n",
    "for idx in range(num_prim_to_be_checked):\n",
    "    primitive = automata.Primitives[idx]\n",
    "    \n",
    "    if not mode:\n",
    "        # KS Model\n",
    "        trajectory_new = create_new_traj_KS(primitive)\n",
    "        csw = CommonRoadSolutionWriter(output_dir=os.getcwd(),              # output directory\n",
    "                                   scenario_id=scenario_id,                 # scenario id\n",
    "                                   step_size=0.1,                           # time step of scenario, typically 0.1\n",
    "                                   vehicle_type=veh_type,     # vehicle type, change if needed\n",
    "                                   vehicle_model=VehicleModel.KS,           # \n",
    "                                   cost_function=CostFunction.JB1)          # \n",
    "                \n",
    "    else:\n",
    "        # PM Model\n",
    "        trajectory_new = create_new_traj_PM(primitive)\n",
    "    \n",
    "    \n",
    "        csw = CommonRoadSolutionWriter(output_dir=os.getcwd(),              # output directory\n",
    "                                   scenario_id=scenario_id,                 # scenario id\n",
    "                                   step_size=0.1,                           # time step of scenario, typically 0.1\n",
    "                                   vehicle_type=veh_type,     # vehicle type, change if needed\n",
    "                                   vehicle_model=VehicleModel.PM,           # vehicle model, change if needed\n",
    "                                   cost_function=CostFunction.JB1)          # cost funtion, change if needed\n",
    "\n",
    "        \n",
    "    # use solution writer to generate target xml file\n",
    "    csw.add_solution_trajectory(trajectory=trajectory_new, planning_problem_id=planning_problem.planning_problem_id)\n",
    "    xmlTree = csw.root_node\n",
    "    \n",
    "    # generate states to be checked\n",
    "    if not mode:\n",
    "        [node] = xmlTree.findall('ksTrajectory')\n",
    "        veh_trajectory = KSTrajectory.from_xml(node)\n",
    "        # first parameter: \n",
    "        veh_model = KinematicSingleTrackModel(veh_type_id, veh_trajectory, None)\n",
    "    else:\n",
    "        [node] = xmlTree.findall('pmTrajectory')\n",
    "        veh_trajectory = PMTrajectory.from_xml(node)\n",
    "        veh_model = PointMassModel(veh_type_id, veh_trajectory, None)\n",
    "        \n",
    "    \n",
    "    list_verification_results = []\n",
    "    output_message = \"\"\n",
    "    \n",
    "    # iterate through each pair of the states within the new trajectory\n",
    "    for (x0, x1, i) in zip(veh_trajectory.state_list[:-1], \n",
    "                           veh_trajectory.state_list[1:], \n",
    "                           range(len(veh_trajectory.state_list) - 1)):\n",
    "        \n",
    "        # transition time between states = 1 second (time for whole trajectory) / (number of states - 1)\n",
    "        # maybe dt should be changed?\n",
    "        \n",
    "        dt = 1 / (len(veh_trajectory.state_list) - 1)\n",
    "        result = TrajectoryValidator.is_state_transition_feasible(x0, x1, dt, veh_model)\n",
    "        list_verification_results.append(result)\n",
    "#         output_message += \"Trajectory from time step {} to {} feasible: {}\\n\".format(i, i + 1, result)\n",
    "\n",
    "    # compute feasible percentage\n",
    "    feasible_percentage = round(sum(list_verification_results) / len(list_verification_results), 1) * 100\n",
    "    \n",
    "    for percentage in list_percentage:\n",
    "        if feasible_percentage == percentage:\n",
    "            dict_feasible_percentage[percentage] += 1\n",
    "            break\n",
    "    \n",
    "    # Print progress\n",
    "    if idx % 100 == 0 and idx:\n",
    "        print(\"Progress: {} primitives checked.\".format(idx))\n",
    "\n",
    "#     print(\"Primitive No.{}: {} / {} states are feasible.\".format(idx, sum(list_check_result), len(list_check_result)))\n",
    "#     print(output_message)\n",
    "\n",
    "# print final output\n",
    "for percentage, count in dict_feasible_percentage.items():\n",
    "    print(\"Feasible percentage: {:>3}  count: {:>3}\".format(percentage, count))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "source": [
    "# Analytic Check of PM Model\n",
    "based on [vehicle model](https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/blob/master/vehicleModels_commonRoad.pdf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "time: 2.95 ms\n"
     ]
    }
   ],
   "source": [
    "from math import sin, cos\n",
    "\n",
    "def satisfies_pm_model(x0, x1, dt, a_max):\n",
    "    # takes in a pair of consecutive states to be checked\n",
    "    \n",
    "    # state of x0\n",
    "    x_x0 = x0.position[0]\n",
    "    y_x0 = x0.position[1]\n",
    "    v_x0 = x0.velocity\n",
    "    o_x0 = x0.orientation\n",
    "    \n",
    "    v_x_x0 = v_x0 * cos(o_x0)\n",
    "    v_y_x0 = v_x0 * sin(o_x0)\n",
    "    \n",
    "    # state of x1\n",
    "    x_x1 = x1.position[0]\n",
    "    y_x1 = x1.position[1]\n",
    "    \n",
    "    # get difference in x and y of the two states\n",
    "    d_x = x_x1 - x_x0\n",
    "    d_y = y_x1 - y_x0\n",
    "    \n",
    "    # compute required input a_x and a_y: d_x = v_x_x0 * dt + 1/2 * a_x * (dt ** 2)\n",
    "    a_x = 2 * (d_x - v_x_x0 * dt) / (dt ** 2)\n",
    "    a_y = 2 * (d_y - v_y_x0 * dt) / (dt ** 2)\n",
    "    \n",
    "#     print(\"x:01d \", x_x0, x_x1, d_x)\n",
    "#     print(\"v:xyv\", v_x_x0, v_y_x0, v_x0)\n",
    "#     print(\"formula and x1 comparison:\", x_x0 + v_x0*dt + 0.5 * a_x * dt ** 2, x_x1)\n",
    "#     print(\"===\")\n",
    "    \n",
    "    if (a_x ** 2 + a_y ** 2) ** 0.5 <= a_max:\n",
    "        return True\n",
    "    else:\n",
    "        return False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Analytic Check of PM model\n",
      "Total number of primitives to be checked:  2984\n",
      "Progress: 100 primitives checked.\n",
      "Progress: 200 primitives checked.\n",
      "Progress: 300 primitives checked.\n",
      "Progress: 400 primitives checked.\n",
      "Progress: 500 primitives checked.\n",
      "Progress: 600 primitives checked.\n",
      "Progress: 700 primitives checked.\n",
      "Progress: 800 primitives checked.\n",
      "Progress: 900 primitives checked.\n",
      "Progress: 1000 primitives checked.\n",
      "Progress: 1100 primitives checked.\n",
      "Progress: 1200 primitives checked.\n",
      "Progress: 1300 primitives checked.\n",
      "Progress: 1400 primitives checked.\n",
      "Progress: 1500 primitives checked.\n",
      "Progress: 1600 primitives checked.\n",
      "Progress: 1700 primitives checked.\n",
      "Progress: 1800 primitives checked.\n",
      "Progress: 1900 primitives checked.\n",
      "Progress: 2000 primitives checked.\n",
      "Progress: 2100 primitives checked.\n",
      "Progress: 2200 primitives checked.\n",
      "Progress: 2300 primitives checked.\n",
      "Progress: 2400 primitives checked.\n",
      "Progress: 2500 primitives checked.\n",
      "Progress: 2600 primitives checked.\n",
      "Progress: 2700 primitives checked.\n",
      "Progress: 2800 primitives checked.\n",
      "Progress: 2900 primitives checked.\n",
      "Feasible percentage: 100  count:   0\n",
      "Feasible percentage:  90  count: 860\n",
      "Feasible percentage:  80  count: 678\n",
      "Feasible percentage:  70  count: 720\n",
      "Feasible percentage:  60  count: 242\n",
      "Feasible percentage:  50  count:  80\n",
      "Feasible percentage:  40  count:  10\n",
      "Feasible percentage:  30  count:   0\n",
      "Feasible percentage:  20  count:   0\n",
      "Feasible percentage:  10  count:   0\n",
      "Feasible percentage:   0  count: 394\n",
      "time: 5.59 s\n"
     ]
    }
   ],
   "source": [
    "# add path to vehicle model (https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/tree/master/Python)\n",
    "sys.path.append(\"/home/edmond/Softwares/commonroad/commonroad-search/GSMP/motion_automata/vehicle_model\")\n",
    "\n",
    "from solution_checker import *\n",
    "import xml.etree.ElementTree as et\n",
    "from math import atan2, sin, cos\n",
    "from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction\n",
    "\n",
    "# 1: FORD_ESCORT 2: BMW_320i 3: VW_VANAGON\n",
    "veh_type_id = 3\n",
    "\n",
    "if veh_type_id == 1:\n",
    "    veh_type = VehicleType.FORD_ESCORT\n",
    "elif veh_type_id == 2:\n",
    "    veh_type = VehicleType.BMW_320i\n",
    "elif veh_type_id == 3:\n",
    "    veh_type = VehicleType.VW_VANAGON\n",
    "    \n",
    "# for feasible percentage computation\n",
    "dict_feasible_percentage = {}\n",
    "list_percentage = [p for p in range(0, 101, 10)]\n",
    "list_percentage.reverse()\n",
    "for percentage in list_percentage:\n",
    "    dict_feasible_percentage[percentage] = 0\n",
    "\n",
    "print(\"Analytic Check of PM model\")\n",
    "\n",
    "num_prim_to_be_checked = len(automata.Primitives)\n",
    "# num_prim_to_be_checked = 50\n",
    "\n",
    "print(\"Total number of primitives to be checked: \", num_prim_to_be_checked)\n",
    "for idx in range(num_prim_to_be_checked):\n",
    "    primitive = automata.Primitives[idx]\n",
    "    \n",
    "    # PM Model\n",
    "    trajectory_new = create_new_traj_PM(primitive)\n",
    "\n",
    "\n",
    "    csw = CommonRoadSolutionWriter(output_dir=os.getcwd(),              # output directory\n",
    "                               scenario_id=scenario_id,                 # scenario id\n",
    "                               step_size=0.1,                           # time step of scenario, typically 0.1\n",
    "                               vehicle_type=veh_type,     # vehicle type, change if needed\n",
    "                               vehicle_model=VehicleModel.PM,           # vehicle model, change if needed\n",
    "                               cost_function=CostFunction.JB1)          # cost funtion, change if needed\n",
    "\n",
    "        \n",
    "    # use solution writer to generate target xml file\n",
    "    csw.add_solution_trajectory(trajectory=trajectory_new, planning_problem_id=planning_problem.planning_problem_id)\n",
    "    xmlTree = csw.root_node\n",
    "\n",
    "    [node] = xmlTree.findall('pmTrajectory')\n",
    "    veh_trajectory = PMTrajectory.from_xml(node)\n",
    "    veh_model = PointMassModel(veh_type_id, veh_trajectory, None)\n",
    "        \n",
    "    \n",
    "    list_verification_results = []\n",
    "    output_message = \"\"\n",
    "    \n",
    "    # iterate through each pair of the states within the new trajectory\n",
    "    for (x0, x1, i) in zip(veh_trajectory.state_list[:-1], \n",
    "                           veh_trajectory.state_list[1:], \n",
    "                           range(len(veh_trajectory.state_list) - 1)):\n",
    "        \n",
    "        # transition time between states = 1 second (time for whole trajectory) / (number of states - 1)\n",
    "        # maybe dt should be changed?\n",
    "        \n",
    "#         dt = 1 / (len(veh_trajectory.state_list) - 1)\n",
    "        dt = 1 / 40\n",
    "        a_max = 11.5\n",
    "        \n",
    "        result = satisfies_pm_model(x0, x1, dt, a_max)\n",
    "        \n",
    "        list_verification_results.append(result)\n",
    "#         output_message += \"Trajectory from time step {} to {} feasible: {}\\n\".format(i, i + 1, result)\n",
    "\n",
    "    # compute feasible percentage\n",
    "    feasible_percentage = round(sum(list_verification_results) / len(list_verification_results), 1) * 100\n",
    "    \n",
    "    for percentage in list_percentage:\n",
    "        if feasible_percentage == percentage:\n",
    "            dict_feasible_percentage[percentage] += 1\n",
    "            break\n",
    "    \n",
    "    # Print progress\n",
    "    if idx % 100 == 0 and idx:\n",
    "        print(\"Progress: {} primitives checked.\".format(idx))\n",
    "\n",
    "#     print(\"Primitive No.{}: {} / {} states are feasible.\".format(idx, sum(list_check_result), len(list_check_result)))\n",
    "#     print(output_message)\n",
    "\n",
    "# print final output\n",
    "for percentage, count in dict_feasible_percentage.items():\n",
    "    print(\"Feasible percentage: {:>3}  count: {:>3}\".format(percentage, count))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Analytic check of PM model, a_max = 11.5, dt = 1 / 40"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python (cr37)",
   "language": "python",
   "name": "cr37"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
